#include "HalCAN.h"
//#include "HalWait.h"

#define HAL_CAN_ID 0x0 //1
#define HAL_CAN_MASK 0x0 //all recv 0xFFE00000

#define CAN_REG_GET_HIGH(x) (((x) >> 16) & 0xffff)
#define CAN_REG_GET_LOW(x)  ((x) & 0xffff)

typedef struct
{
    CAN_RxHeaderTypeDef header;
    uint8_t data[8];
}HalCANPacket_t;

#pragma pack(1)
typedef struct
{
    uint8_t reserve:1;
    uint8_t RTR:1;
    uint8_t IDE:1;
    uint32_t EXID:18;
    uint16_t STID:11;
}HalCanFilterReg_t;
#pragma pack()

typedef union
{
    uint32_t value;
    HalCanFilterReg_t reg;
}HalCANFilter_t;

extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;

static HalCANPacket_t g_packet;
static HalCANRecvData_cb g_dataRecvCb;

static int filterConfigs(void)
{
    int ret;
    HalCANFilter_t filter = {0};

    filter.reg.IDE = 0;
    filter.reg.RTR = 0;
    filter.reg.STID = HAL_CAN_ID;

    CAN_FilterTypeDef  can1Config;

    can1Config.FilterIdHigh     = CAN_REG_GET_HIGH(filter.value);
    can1Config.FilterIdLow      = CAN_REG_GET_LOW(filter.value);
    can1Config.FilterMaskIdHigh = CAN_REG_GET_HIGH(HAL_CAN_MASK);
    can1Config.FilterMaskIdLow  = CAN_REG_GET_LOW(HAL_CAN_MASK); 
    can1Config.FilterMode       = CAN_FILTERMODE_IDMASK;
    can1Config.FilterScale      = CAN_FILTERSCALE_32BIT;
    can1Config.FilterBank       = 0;
    can1Config.FilterFIFOAssignment = CAN_RX_FIFO0;
    can1Config.FilterActivation     = ENABLE;
    can1Config.SlaveStartFilterBank = 0;
    ret = HAL_CAN_ConfigFilter(&hcan1, &can1Config); //can1
    
    can1Config.FilterBank = 14;
    can1Config.SlaveStartFilterBank = 14;
    ret |= HAL_CAN_ConfigFilter(&hcan2, &can1Config); //can2

    return ret == HAL_OK ? 0 : -1;
}

static int lowSend(CAN_HandleTypeDef * phcan, uint32_t id, uint8_t *data, uint8_t len)
{
    CAN_TxHeaderTypeDef header;
    uint32_t mailbox;
    int ret;

    header.DLC   = len;
    header.StdId = id;
    header.IDE   = CAN_ID_STD;
    header.RTR   = CAN_RTR_DATA;
    header.TransmitGlobalTime = DISABLE;

    ret = HAL_CAN_AddTxMessage(phcan, &header, data, &mailbox);
    return ret == HAL_OK ? len : 0;
}

int HalCANSendWaitAck(HalCANPort_t port, uint32_t id, uint8_t *data, uint8_t len, uint8_t *recvBuff)
{
    CAN_HandleTypeDef * phcan = port == HAL_CAN_PORT_1 ? &hcan1 : &hcan2;
    HAL_CAN_DeactivateNotification(phcan, CAN_IT_RX_FIFO0_MSG_PENDING);
    if(lowSend(phcan, id, data, len) < 0)
    {
        return -1;
    }

    uint16_t retry = 0;
    CAN_RxHeaderTypeDef header;
    while (1)
    {
        //HalWaitUs(50);
        if(HAL_CAN_GetRxFifoFillLevel(phcan, CAN_RX_FIFO0))
        {
            HAL_CAN_GetRxMessage(phcan, CAN_RX_FIFO0, &header, recvBuff);
            break;
        }
        retry++;
        if(retry > 3000) //2ms
        {
            return 0;
        }
    }

    return header.DLC;
}

int HalCANSend(HalCANPort_t port, uint32_t id, uint8_t *data, uint8_t len)
{
    CAN_HandleTypeDef * phcan = port == HAL_CAN_PORT_1 ? &hcan1 : &hcan2;
    //HAL_CAN_ActivateNotification(phcan, CAN_IT_RX_FIFO0_MSG_PENDING);
    return lowSend(phcan, id, data, len);
}

int HalCANInitialize(HalCANRecvData_cb recvCallback)
{
    int ret;
    if(filterConfigs() < 0)
    {
        return -1;
    }
    ret = HAL_CAN_Start(&hcan1);
    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
    
    ret |= HAL_CAN_Start(&hcan2);
    HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING);
    g_dataRecvCb = recvCallback;
    return ret == HAL_OK ? 0 : -2;
}

void HalCANPoll(void)
{
}
#if 0
static void canRecvHandle(HalCANPort_t port, uint32_t id, uint8_t *data, uint8_t len)
{
    uint8_t i;
    CAN_HandleTypeDef * phcan = port == HAL_CAN_PORT_1 ? &hcan1 : &hcan2;
    
    printf("CAN%d:recv len = %d, id = 0x%x, data = [", port+1, len, id);
    for(i = 0; i < len; i++)
    {
        printf("0x%02x ", data[i]);
    }
    printf("]\n");
    
    if(g_dataRecvCb)
    {
        g_dataRecvCb(port, id, data, len);
    }
    HAL_CAN_ActivateNotification(phcan, CAN_IT_RX_FIFO0_MSG_PENDING);
}
#endif

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *phcan)
{
    HalCANPort_t port;
    bool gotPacket = false;
    uint32_t id;
    
    if(phcan->Instance == hcan1.Instance)
    {
        if(HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &g_packet.header, g_packet.data) == HAL_OK)
        {
            gotPacket = true;
            port = HAL_CAN_PORT_1;
        }
    }
    else if(phcan->Instance == hcan2.Instance)
    {
        if(HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO0, &g_packet.header, g_packet.data) == HAL_OK)
        {
            gotPacket = true;
            port = HAL_CAN_PORT_2;
        }
    }
    
    if(gotPacket)
    {
        id = g_packet.header.IDE == CAN_ID_STD ? g_packet.header.StdId : g_packet.header.ExtId;
        // canRecvHandle(port, id, g_packet.data, g_packet.header.DLC);
        g_dataRecvCb(port, id, g_packet.data, g_packet.header.DLC);
        HAL_CAN_ActivateNotification(phcan, CAN_IT_RX_FIFO0_MSG_PENDING);
    }
}


